//
// Created by FindBin on 2024/6/15.
//

#include "bsp_can.h"
#include "bsp_fbcf.h"

#define fbcf_base_id (0x6f0)
#define fbcf_id(x) (fbcf_base_id | x)


static void fbcf_send_msg(uint16_t id, uint8_t *pDate) {
    fdcan_send_msg(&FBCF_CAN, fbcf_id(id), FDCAN_DLC_BYTES_8, pDate);
}

void fbcf_arm(uint16_t id) {
    static uint64_t command = (uint64_t) (0xfbeebfaa);
    fbcf_send_msg(id, (uint8_t *) (&command));
}

void fbcf_set_pos(uint16_t id, float32_t angle) {
    FedSetMaster_t fedSetMaster = {
            .pos = angle,
            .spd = 0.f
    };
    fbcf_send_msg(id, (uint8_t *) &fedSetMaster);
}

void fbcf_set_spd(uint16_t id, float32_t spd) {
    FedSetMaster_t fedSetMaster = {
            .spd = spd,
            .pos = 0.f,
    };
    fbcf_send_msg(id, (uint8_t *) &fedSetMaster);
}

void fbcf_set_pos_spd(uint16_t id, float32_t pos, float32_t spd) {
    FedSetMaster_t fedSetMaster = {
            .pos = pos,
            .spd = spd,
    };
    fbcf_send_msg(id, (uint8_t *) &fedSetMaster);
}